/*
    <one line to give the program's name and a brief idea of what it does.>
    Copyright (C) 2013  <copyright holder> <email>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/


#include "OCTLandmarkLocating.h"

OCTLandmarkLocating::OCTLandmarkLocating()
{
  
}

OCTLandmarkLocating::~OCTLandmarkLocating()
{

}

bool OCTLandmarkLocating::locatingLandmark(Landmark* landmark, LandmarkPattern* pattern, Point3f& loc)
{
  
  //cout << ERROR_FILE_NOT_FOUND << endl;
  OCTImageProcessing* imgP = new OCTImageProcessing();
  
  // fit the plane of the input surface and subtract it
  Mat searchRegion = imgP->surfaceSubtraction(landmark->getLandmarkSource());
  //searchRegion = imgP->convertSurfaceToGrayImage(searchRegion);
  
  // Gaussian filter
  Mat gaussM;
  GaussianBlur(searchRegion, gaussM, Size(5,5), 0, 0);

  // 1st derivative
  Mat sobelV, sobelH, sobelM, absSobelH, absSobelV;
  Sobel(gaussM, sobelH, CV_32FC1, 1, 0, 3);
  Sobel(gaussM, sobelV, CV_32FC1, 0, 1, 3);

  absSobelH = abs(sobelH);
  absSobelV = abs(sobelV);
  sobelM = absSobelH + absSobelV;
  
  // 2nd derivative
  Mat lapM;
  Laplacian(gaussM, lapM, CV_32FC1);
  for (int i = 0; i < lapM.cols; i++)
    for (int j = 0; j < lapM.rows; j++)
    {
      if (lapM.at<float>(j,i) > 0)
	lapM.at<float>(j,i) = 0;
    } 
  
  // product of 1st and 2nd derivative
  Mat boundary(sobelM.size(), sobelM.type());
  
  //boundary = lapM.mul(sobelM);
  multiply(lapM, sobelM, boundary);
  //lapM.mul();
  multiply(lapM, sobelV, sobelV);
  multiply(lapM, sobelH, sobelH);

  convertScaleAbs(boundary, boundary);
  RotatedRect rect;
  if (detectEllipse(boundary, landmark, pattern, sobelV, sobelH, rect)) {
    loc = calculateCentroid(landmark->getLandmarkSource(), rect, landmark);
    if (DEBUG)
      cout << "Landmark centroid : " << loc << endl;
  } else {
    cout << "Can not detect the landmark" << endl;
    return false;
  }
  
  return true;
}

bool OCTLandmarkLocating::detectEllipse(Mat boundary, Landmark* landmark, LandmarkPattern* pattern, 
					Mat sobelV, Mat sobelH, RotatedRect& rotatedRect)
{
  /// determine the appropriate threshold for boundary points  
  normalize(boundary, boundary, 0, 255, NORM_MINMAX, CV_32FC1);
  Mat imgBuffer = boundary.reshape(0,0);
  Scalar tempVal = mean(imgBuffer);
  float thr = tempVal.val[0];
  
  float deltaThr = 255;

  for (int i = 0; i < imgBuffer.cols * imgBuffer.rows; i++)
  {
    //
    float t1 = 0;
    float c1 = 0;
    float t2 = 0;
    float c2 = 0;
    if (imgBuffer.at<float>(0,i) > thr)
    {
      t1 += imgBuffer.at<float>(0,i);
      c1++;
    }
    else
    {
      t2 += imgBuffer.at<float>(0,i);
      c2++;
    }
    if (c1 != 0 && c2 != 0)
    {
      float thrOld = thr;
      thr = 0.5 * (t1 / c1 + t2 / c2);
      deltaThr = thr - thrOld;
    }
    
    if (deltaThr <= 1)
      break;
  }

  /// apply the threshold
  for (int i = 0; i < boundary.cols; i++)
    for (int j = 0; j < boundary.rows; j++)
    {
      if (boundary.at<float>(j,i) < thr)
	boundary.at<float>(j,i) = 0;
      if (boundary.at<float>(j,i) != 0)
	boundary.at<float>(j,i) = 1;
    }
  

  
  multiply(sobelH, boundary, sobelH);
  multiply(sobelV, boundary, sobelV);

  
  /// determine the cross correlation matrix
  /// using matchTemplate method
  /// crossCorr.cols = sobel.cols - pattern.cols + 1
  Mat crossCorrH, crossCorrV, crossCorr; 
  matchTemplate(sobelH, pattern->getPatternH(), crossCorrH, CV_TM_CCORR);
  matchTemplate(sobelV, pattern->getPatternV(), crossCorrV, CV_TM_CCORR);
  add(crossCorrH, crossCorrV, crossCorr);
  
  /// detect the point with the max correlation and treat as the center of the landmark
  Point center;
  int radius = pattern->getRadius();
  int offset = radius / 10;
  int patternCols = pattern->getPatternMatrix().cols;
  int patternRows = pattern->getPatternMatrix().rows;

  minMaxLoc(crossCorr, NULL, NULL, NULL, &center);
  
  if (DEBUG)
    cout << "Max correlation at : " << center << endl;
  
  if (center.x == 0 && center.y == 0){
    // Error condition
    return false;
  }
  
  
  Mat centerM = Mat::zeros(1, 2, CV_8UC1);
//   centerM.at<uchar>(0,0) = (uchar) center.x - radius - offset;
//   centerM.at<uchar>(0,1) = (uchar) center.y - radius - offset;
//   
  centerM.at<uchar>(0,0) = (uchar) center.x + patternCols/2;
  centerM.at<uchar>(0,1) = (uchar) center.y + patternRows/2;
  
  
  /// set the point outside the tolerance band to zero
  float tolerance = radius * 0.4;
  float maxAngle = M_PI/6;
  float angleThr = maxAngle;
  normalize(boundary, boundary, 0, 255, NORM_MINMAX);
  
  if (SHOWIMG)
  {
    namedWindow("Boundary");
    imshow("Boundary", boundary);
  }
  
  int threshold = 255;
   
  ///
  Mat centerPreM = Mat::zeros(1, 2, CV_8UC1);
  centerPreM.at<uchar>(0,0) = centerM.at<uchar>(0,0) + 10;
  centerPreM.at<uchar>(0,1) = centerM.at<uchar>(0,1) + 10;
  
  //RotatedRect rtn;
  bool locEllipse = false;
  
  while(true)
  {
    if (norm(centerPreM - centerM) < 0.25)
      break;
    vector<Point> contour;
    for (int i = 0; i < boundary.cols; i++)
      for (int j = 0; j < boundary.rows; j++)
      {
	//
	float r;
	r = sqrt((i - centerM.at<uchar>(0,0))*(i - centerM.at<uchar>(0,0)) 
	      + (j - centerM.at<uchar>(0,1))*(j - centerM.at<uchar>(0,1)) );
	Mat dirVec = Mat::zeros(1, 2, CV_32FC1);
	Mat gradVec = Mat::zeros(1, 2, CV_32FC1);
	
	dirVec.at<float>(0,0) = i - centerM.at<uchar>(0,0);
	dirVec.at<float>(0,1) = j - centerM.at<uchar>(0,1);
	
	gradVec.at<float>(0,0) = sobelH.at<float>(j,i);
	gradVec.at<float>(0,1) = sobelV.at<float>(j,i);
	
	float angle;
	if (norm(dirVec) > 0 && norm(gradVec) > 0)
	{
	  normalize(dirVec, dirVec);
	  normalize(gradVec, gradVec);
	  angle = acos(dirVec.dot(gradVec));
	  
	}
	else
	  continue;
	
	if (r <= tolerance + radius && r >= (radius) * cos(maxAngle) - tolerance 
	      && angle < angleThr)
	{
	  Point tempPoint;
	  tempPoint.x = i;
	  tempPoint.y = j;
	  contour.push_back(tempPoint);
	}
      }
    
    if (contour.size() < 4)
    {
      cout << "Contour Size: " << contour.size() << "\t" << ERROR_ELLIPSE_POINTS_LESS << endl;
      break;
    }
    
    rotatedRect = fitEllipse(contour);
    centerPreM.at<uchar>(0,0) = centerM.at<uchar>(0,0);
    centerPreM.at<uchar>(0,1) = centerM.at<uchar>(0,1);
    
    centerM.at<uchar>(0,0) = rotatedRect.center.x;
    centerM.at<uchar>(0,1) = rotatedRect.center.y;
    
    locEllipse = true;
  }
  
  if (!locEllipse)
    return false;
  
  if (DEBUG)
    cout<< "Ellipse : " << rotatedRect.center << "\t" << rotatedRect.size.width << "\t" << rotatedRect.size.height << endl;
 
  return true;  
}

Point3f OCTLandmarkLocating::calculateCentroid(Mat landmarkSurface, RotatedRect ellipseRect, Landmark* landmark)
{
  Mat tmp = Mat::zeros(landmarkSurface.size(), CV_8UC1);
  ellipse(tmp, ellipseRect, Scalar(255), -1);
  uchar white = 255;
  float x = 0;
  float y = 0;
  float z = 0;
  int counter = 0;
  
  /// Calculate the points centroid inside the ellipse
  for (int i = 0; i < landmarkSurface.cols; i++)
    for (int j = 0; j < landmarkSurface.rows; j++)
    {
      //
      if (tmp.at<uchar>(j,i) == white)
      {
	x += i;
	y += j;
	z += landmarkSurface.at<float>(j,i);
	counter++;
      }
    }
  
  int startRow = landmark->getLandmarkCenter().y - landmark->getLandmarkRange();
  int startCol = landmark->getLandmarkCenter().x - landmark->getLandmarkRange();
  
  x = x/counter;
  y = y/counter;
  z = z/counter;
  
  Mat colorTmp;
  cvtColor(landmark->getLandmarkImage(), colorTmp, CV_GRAY2RGB);
  ellipse(colorTmp, ellipseRect, Scalar(255,255,0), 1);
  line(colorTmp, Point(x - 10, y), Point(x + 10, y), Scalar(255,0,0) );
  line(colorTmp, Point(x, y - 10), Point(x, y + 10), Scalar(255,0,0) );
  
  imwrite(landmark->getFileName(), colorTmp);
  
  x += startCol;
  y += startRow;
   
  return Point3f(x,y,z);
  
}